use cmsis_rtos2_rs::os_delay;
use driver_utils::service::motor::Dir;
use driver_utils::service::motor::Motor;
use driver_utils::service::motor::MotorDrv;
use periph_utils::AdcInit;
use periph_utils::DmaConfig;
use periph_utils::GpioConfig;
use periph_utils::stm32f1;
use periph_utils::PeriphReg;
use utils::UninitCell;

use crate::cp_door::DoorErr;

use super::sensor;


#[derive(GpioConfig, AdcInit, DmaConfig)]
#[gpio(init = "gpio_init",
    io(name = "dir", io = "PB12", mode = "OUT_PP", set, reset),
    io(name = "r", io = "PB13", mode = "OUT_PP", set, reset),
    io(name = "pick_door", io = "PB14", mode = "OUT_PP", set, reset),
    io(name = "push_motor", io = "PB15", mode = "OUT_PP", set, reset),
    io(name = "analog", io = "PC2", mode = "AIN")
)]
#[adc_config(adc = 1, chs = [12])]
#[dma(init = "dma_init", 
    ch(name = "adc", dma = 1, ch = 1, size = 16, circ, dir = "read")
)]
struct Init;

#[derive(PeriphReg)]
#[periph(periph = "ADC1")]
struct ADC;

static ADC_DATA_BUF: UninitCell<[u16; 1]> = UninitCell::const_new();

pub unsafe fn init() {
    Init::gpio_init();
    Init::dma_init();
    Init::adc_init();
    Init::adc_dma1_ch1_read(
        ADC_DATA_BUF.as_ptr() as _,
        ADC.dr.as_ptr() as _,
        ADC_DATA_BUF.len() as _,
    );
    Init::adc_start();
}

pub fn analog_value() -> u16 {
    ADC_DATA_BUF[0]
}

type PickDoor = Motor<PickDoorCtrl>;

pub fn pick_door_open() -> Result<(), DoorErr> {
    PickDoor::run_to_ccw_origin()
}

pub fn pick_door_close() -> Result<(), DoorErr> {
    PickDoor::run_to_cw_origin()
}

struct PickDoorCtrl;

impl MotorDrv for PickDoorCtrl {
    fn analog_value() -> u16 {
        analog_value()
    }

    fn delay(tick: u32) {
        os_delay(tick);
    }

    fn is_ccw_origin() -> bool {
        sensor::is_pick_door_open()
    }

    fn is_cw_origin() -> bool {
        sensor::is_pick_door_close()
    }

    fn set_dir(dir: Dir) {
        match dir {
            Dir::Brake => {
                Init::dir_reset();
                Init::pick_door_reset();
            },
            Dir::CCW => {
                Init::dir_set();
                Init::pick_door_set();
            },
            Dir::CW => {
                Init::dir_reset();
                Init::pick_door_set();
            }
        };
    }

    type Error = DoorErr;
    const ALM_ERR: Self::Error = DoorErr::PickDrorAlm;
    const TIMEOUT_ERR: Self::Error = DoorErr::PickDoorTimeout;
}


